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I.  Introduction 

The  Rapid  Gravity  Survey  System  (RGGS)  provides  a means 
of  quickly  measuring  precisely  non-astrogeodetlc  values  of  the 
deflections  of  the  vertical.  A test  vehicle  carries  an  Inertial 
Positioning  System  (IPS)  which  at  each  of  the  vehicle's  stops 
produces  an  error  velocity  which  can  be  related  to  the  Inertial 
platform  tilt  errors  and  the  deflections  of  the  vertical.  An 
optimal  determination  of  the  gyro  drifts  and  the  deflections  of 
the  vertical  can  only  be  obtained  by  a post-mission  smoothing  of 
the  data.  In  this  case,  accurate  data  are  available  a priori 
for  the  deflections  of  the  vertical  at  the  start  and  stop  of  the 
vehicle's  mission  — as  well  as  the  data  on  the  IPS  velocity  errors 
at  each  stop. 

The  purpose  of  this  report  Is  to  develop  the  equations  for 

the  position,  velocity,  and  tilt  angle  errors  into  a useable  algorithm  for 

the  optimal  estimation  of  the  deflections  of  the  vertical.  This  entails 

three  major  analytic  tasks  that  will  be  presented  in  the  succeeding 

section:  one,  the  development  of  an  expression  for  the  velocity  errors 

at  some  time,  t , In  terms  of  the  known  Initial  conditions  at  time, 
n 

t ■ 0,  and  the  unknown  variables  to  be  estimated  — i.e.  the  gyro  drift 
biases  and  the  values  of  and  P^,  the  vertical  deflections  at  previous 
time  Intervals;  tvo,  the  replacement  of  and  p$  by  a smaller  .number  of 

A * 

values  and  p^  — the  needed  values  of  Cf  and  p£  being  produced  by 

A A % 

statistical  collocation  from  the  p^  basis  set;  three,  a least 
squares  solution  for  the  gyro  biases  and  the  1^,  p^  that  minimizes  the 
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] 


p 
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ne«n  square  deviation  of  the  velocity  errors  from  their  estimates. 

This  mathematical  development  allows  the  production  of  a machine 
algorithm  for  use  with  actual  data.  A coded  version  of  the  algorithm  with 
explanatory  comments  will  make  up  the  final  section  of  this  report. 


II.  Mathematical  Development  of  the  Error  Adjustment  Technique 

The  differential  equations  for  the  horizontal  velocity  errors  are 


du  m dk  m 
dt  dt 


-SE*Z  + 8*E  + 85 


(1) 
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dx 

dt 
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cv  • 
dt  dt 


- Vz  “ 8*N  + 8n 
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d*Z  - 
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where  the  meaning  of  the  symbols  is  as  follows: 

* north  position  error 
north  velocity  error 
north  vehicle  position 
north  vehicle  velocity 
north  vehicle  acceleration 
east  position  error 
east  velocity  error 
east  vehicle  position 
east  vehicle  velocity 
east  vehicle  acceleration 
azimuth  plavform  uttitu*  error 
platform  tilt  Hc-or  t*bout  nct^h  axis 
platform  tilt  ^ about  !a»c  axis 
geographic  *._■  .tu«.‘«» 
normal  grav  Aty 
mean  earth  radius 
earth's  angular  rotation  rate 
Schuler  rate  = ( g/R  )2^2 
ft  cos  f + Vg/R  north  spatial  rate 
5 east  8Patial  rata 

s ft  sin  f + tan  f Vg/R  vertical  spatial  rate 
asimuth  axis  error  angular  drift  rate 
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6 north  axis  error  angular  drift  rate 
Y east  axis  error  angular  drift  rate 
€ north  deflection  of  the  vertical 
n‘  east  deflection  of  the  vertical 


Except  for  the  terms  In  (1)  and  (3)  Involving  S and  S™,  this 
set  may  be  solved  In  a straightforward  manner.  These  restrictions  are 
not  really  llmltlngv  as  we  will  show  below.  The  reduced  set  of  equations 
Is 


du  g*_  - g£  (7) 

dt  * E 


dx 

dt 


(8) 


dt  " “g*N  + W (9) 

d*Z  . 2 £ 

“dt  tan  f R - 0 cos  f R + 0 cos  f + a (10) 


d*N  . 2 - n sin  f ♦ + $ 

lt~  R E 


(11) 


r 

1 , 


dt 


***!  ' - * LJJ4P  f 


R + ft  aln  f ^ - ft  cos  f #E  + y»  (12) 


where  we  have  neglected  terns  in  V/R  relative  to  the  earth  rotation 
rate,  ft  . This  Is  also  the  rationale  for  dropping  — at  least  Initially 
«—  the  terms  containing  SN  and  S£  which  are  of  order  vehicle  accele- 
ration/ g relative  to  the  leading  terns. 

Equations  (7)  - (12)  are  of  the  form 

d u A u ■ T (13) 

dt 

where  A Is  the  coefficient  matrix. 
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Assume  that  the  solution  to  the  homogeneous  part  of  (13)  has  a 
functional  dependence,  e*c.  Then,  the  homogeneous  part  of  (13)  Is 

(14) 


3 

ji 

j 
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( X - A)  U 
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a standard  eigenvalue  problea.  The  secular  equation,  Det(X-A)-0 
gives,  as  Its  roots,  the  values  of  X for  which  (14)  Is  satisfied. 
The  secular  equation  Is 


X 

0 

-1 

0 

0 

1 

R 


0 

X 

0 

-tan  9 

-ir-* 

-l 

R 

0 


0 0 0 -g 

0 0 +g  0 

X 0 0 0 

Qcos  f X 0 -flcosp 

0 0 X Osin  f 

0 +flco8  f -flsinp  X 


■ 0 (15a) 


x6  + x4(2n2  + n2)  + x2(n4  + n2o2sin2  ?)  - n2n4cos2  f - o (15b) 

9 8 ® ® 


which  has  roots 


where 


x - ±m8,  tlfl‘  ,±K 

Jl2  • ( )*^2  the  Schuler  frequency, 

n'  - [ i (n2  +n2)  + i { n4  + 2n2n2  (l  + 2cos2p)  + n4}*/2]1/2 

1 8 7 8 8 (16) 

* { n2  + n2  (l  + cos2p))1/2 
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and 


k ■ (i  { n4  + 2n2n2  (l  + 2cos2#)  + q4)1/2 
2 s • 


3.1  31 


The  eigenvectors  may  be  found  by  solving  (14)  for  all  the  other  elements 
of  p in  terms  of  one  element,  using  a specific  eigenvalue.  The 
eigenvectors  are 


(i)  . 


g Osin  f 
A2  (A2  + a2  j 

- {(xj  + fl*)2  + Xjn2sin  f } /A^  cos  f 

- A^flsinp 


(18) 


(!) 

refers  to  the  eigenvector  associated  with  the  i - th  eigenvalue. 
It  is  also  convenient  to  have  the  eigenvectors  of  the  adjoint  problem, 
I . e • , 

yT  (X  - A)  - 0 

which  has  the  same  eigenvalues  as  (14).  The  eigenvectors  here  are 

PT(i)  -[a"1  t(x|  + a2)  (Q2  cos2f  - xJ)/R, 

2xJ  a sin  p/R,  (19a) 

a2cosVt  (xj  + a2)/R, 

-a  cos  f x2  (x2  + a2) 

Qsihy  a*  (X2  - a2  ) 
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ii  <xJ  + i >] 


where 


*i  “ 4Xin2flg  sl°2f  + 2(x|  + ng  > <XJ  + °Znij  co*Zf)/Xi 


2n2 


(19b) 


Note  that  y^^  is  a row  vector,  and  the  y's  are  normalized  such  that 


pT(l)  . p(j)  « j . We  now  diagonalize  (13)  by  applying  the 

*•»  v-  ij 


transformation 


i - !»  • “i ' *«  "j 


where  S ■ y 
ij  J 


T(i) 


and  S 


-1  - U(J) 


IJ 


since  y**1*  yj1^  - «lfc. 


Equation  (13)  becomes 


d<}l 

dt  ~ Xiqi  “ ri 


(20) 


Xie 


where  r ■ ST^  with  a solution,  • q^  ■ d^e  - r^/X^ 


(21) 


where  c^  is  a constant.  The  boundary  conditions  at  t ■ 0 imply 


ci  " qi  * t- + ri^Xi»  and 

<tt(t)  ■ qi(t«0)  e 1 + Tj/Xj  («  1 - 1) 


(22) 


To  get  back  into  the  original  basis,  wa  apply  the  Inverse  transform  to 
(22)  and  have 

V U 

X«t  . . X,t 


X.t 


X,t 


li<t)  " siJ  <e  J qj(t"0)  + r <e  j •1)rj) 

" Sij  * i sJkpk(t"0)  + Sij  i *1)SJk  *k  * 


(23) 
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which  completes  the  solution  of  the  reduced  set  (7)  - (12). 

Returning  now  to  the  neglected  terms  Involving  and  Sg,  we 
try  a perturbation  addition  to  the  solution  (23).  Thus,  for  example, 
u(t)  - uQ(t)  + ux(t)  + . . . 
where  un  is  found  from  (22) , and  evidently 


du. 


dt 


• -V*o(t>  ’ or 


(24) 


«<t)  - «„<«>-  }oSE*.o<‘'>dt' 

Equation  (24)  could  be  integrated  numerically  given  the  functional 
form  of  Sg(t).  However,  the  integration  time  is  expected  to  be  a 
few  minutes  (3  - 5)  and  the  typical  time  scale,  1/Cl^ , for  variation  of 
the  solution  is  ” 1/2  hour.  This  suggests  trying  a Taylor  series  for 

t'n 

u(t)  - u (t)  - fj  S f *<n)  nf  dt'  . 

0 JO  E n-0 


♦,  . or 

*0 


Keeping  the  first  two  terms  in  the  expansion  and  Integrating  by  parts 


* 


where  Y (t)  is  the  east  position  of  the  vehicle  end  we  assume  that  the 

vehicle  is  stopped  at  time  t.  If  we  differentiate  and  approximate 

d*  d* 

by  fl  4^  since  *0  < 08$x  » we  can  compare  the 


size  of  the  two  terns  in  (25) , and  find 
du0 

— g*E 

dt  E g/R 


w, 


Vm 


or  the  same  order  as  terms  thrown  out  earlier.  However,  we  wish  the 
equations  for  u and  v to  be  as  accurate  as  possible,  since  they  are 
the  ones  that  enter  directly  iuto  the  determination  of  the  deflections 
of  the  vertical.  A further  improvement  on  (25)  can  be  gained  by 

expanding  the  Taylor  series  for  ♦_  about  the  mid-point  of  the  integration 

t2  * 0 

interval.  Then,  the  -=■  term  will  Integrate  to  near 

2 dt2 

zero,  because  the  acceleration  of  the  vehicle  will  be  close  to  an  odd 

function  about  the  midpoint.  In  this  case  (25)  is 

i XJt/2 

u(t)  - uQ(t)  + (Y(t)  - Y(0))  {Sjyje  Sjkwk(t-0) 


i X1t/2 

+ s;  j • 3 v 


where  the  subscript  indicates  the  index  required  to  couple  to 

the  variable,  ♦ . A similar  expression  can  be  derived  for  v(t).  The 
* 

full  solution  can  be  written  as 


Pt(t) 


X,t/2 


" ISiJ  e J SJk  + uijSJlXle  1 hk*vk<* 


, X.t  X-t/2 

♦ <sij('_i)sjk  + “1}V  slk>  \ 


■ * V‘>  \ 


where  u^  ■ 


0 ii*  u,v  and  Jlty 

* 

(Y(t)  - Y(0»  i-u,  J-* 

s 

-C5(t)  -X(0))  i-v,  j-*s 


So  far  we  have  been  concerned  with  the  time  evolution  of  the  variables 
while  the  vehicle  is  moving.  The  vehicle  makes  frequent  stops,  however. 
During  these  stops  the  gyro  angular  errors,  $z,  #N,  ♦g  keep  evolving, 

while  the  other  variables  are  held  constant  by  flat.  To  describe  the 
evolution  of  angle  errors,  we  extract  the  relevant  equations  from 
(7)  - (12)  and  find 


“3t  " 0 cos  f ♦-  + [«  - Q cosp  | ) 


- -Q  sin  f 4g  + 0 


- flsin*N  - a COS  f + Y 


where  u - v -0  identically  and  since  x is  constant,  we  have 
lumped  it  with  ° in  (28).  The  secular  equation  of  (28)  - (30)  is 

v (v2'+  Q2)  - 0 

with  roots  v « 0,  v ■ ±10 

and  transformation  matrices 


cos  e 

72 

cos  p 

“72 

sin  f 


sin  9 

“7* 

sin  9 

“72 

COS  f 


(31a) 


and  «1 
S' 


COS  9 - COS  9 

“7T  ~7z 


sin  9 
"72 

-i 

72 

1 

~72 

cos  f 


(31b) 
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-1 

where  the  rows  in  S'  and  the  columns  in  S'  are  the  eigenvectors 
corresponding  to  eigenvalues  ifl,  -in,  0,  respectively.  Referring 
to  (23),  we  see  that  the  solution  of  (28)  - (30)  is 

»;<«  ■ s'i)  «Vjt  <t-0) 

* *£  v,  <eV  -»>  \ <32> 


*Z 

x 

a - 0 cos  f g 

where  y'  ■ 

♦h 

and  T'  - 

B 

Y 

r « 

and  1_  (e  J - 1)  can  be  understood  for  v ■ 0 as  lira  - t . 

vj  V4° 

We  rewrite  solution  (32)  in  terras  of  all  of  the  variables,  i.e. 

« • 

*«i  - T (t)  Vj(t-O)  + &lj(t-0)  Tj  (33a) 

with 


i V 

* sjk 


k ■ ^2»  ♦jj*  ^E 


(33b) 


1 * H'  *N»  *E 
k - x 


0 elsewhere 


Sljlvj  (C  J " 15  SJk  1*  k " V *H»  *E 


(33c) 


0 elsewhere 


During  the  course  of  tine  during  vhlhc  the  vehicle  is  first  stopped 
for  a time  dr  and  then  in  notion  for  a tine  t,  the  evolution  of 
y is  given  by 


yt(T  + $t)  - ^(r)  Tjk(dT)  wk(t-0)+I*1J(T)A;Jk(6T)  + Aik<T)l  Tk 


• *ik  Mk  (t-0)  + Alk(t,«T)Tk  (34) 


where  *^k  and  A^k  are  defined  by  (34).  We  will  refer  to 

A A 

*(t,6t)  and  A(t,6t)  as  tine  shift  operators  since  they  have  the 
effect  of  updating  y(t)  to  y(T,T,6t)  for  any  t . 

A* 

What  we  wish  to  do  is  to  express  the  observed  quantities  at 

sons  time  to  the  unknown  gyro  biases,  a,  6 , and  y and  unknown 

vertical  deflections.  Let  us  denote  the  value  of  y at  time  t as 

— n 

Vn,  and  let  * (t_  dt  .)  s i*1"1  where  t -t  . « t +dt  . . 

- - ni  n-1  - n b-1  n-1  n-1 


Then,  for  example 


y1  - ♦*  y*+  A*  T* 


y2  ■ ^y1  + * 


A<  A-  _ M A*  _ A 1 I 

- rVp*  ♦ »lA  !'  + A T 


and  in  general 

- n I1  u*  + £ n + Anrn 

- 1-0  - J»0  i-j+1  — *-  ~ ~ 


In  (35)  the  value  of  v and,  in  particular  u and  v are  seen  to 
depend  only  on  the  quantities  for  which  we  wish  to  solve  and  on  the 
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Initial  boundary  conditions.  Three  further  points  need  to  be  made. 

One,  the  development  so  far  has  assumed  that  the  initial  values  for 
the  succeeding  time  steps  are  simply  those  of  the  end  values  of  the 
preceding  time  step.  In  fact,  however,  at  each  vehicle  stop  the  velocity 
errors,  u and  v,  are  set  to  zero,  and  the  integration  continues  with 
u - v * 0 as  a starting  condition.  These  boundary  conditions  can  be 

A 

Incorporated  by  simply  replacing  the  ♦_  operator  by 

*ik  " *ik(6lk  “ Mlk) 

where 

Mlfc  - 0 l^k;  1,  ki*u,  v 

1 l-k-u,v 

So  finally  we  have 

pn+1  . S flu*  + °E1  8 «■  Anfn  (36) 

- 1-0  ~ ~ j-0  1-3-0  ~ 


Two,  the  f11  vectors  are,  In  general,  a function  of  time  — at 
least  parametrically  through  the  motion  of  the  vehicle.  In  order  to 
achieve  best  accuracy,  we  should  time  center  the  values  taken  to  repre- 
sent f . Thus,  for  the  gyro  drift  rates  an-1  - a(t-(tQ+j+tn)/ 2) 
gives  best  accuracy.  The  vertical  deflections  do  not  couple  Into  the 
evolution  of  the  system  while  the  vehicle  is  stopped,  so  that  the 

optimum  here  is  - C(t  - t - r «/2).  Since  the  vehicle  motion 

n+1  n-i 

is  probably  near  to  being  symmetric  about  this  time,  we  may  write 

_ — . . - — - ■ -----  - - - 


• C(£E°+  [Yn  + *n~l]/2)  as  a more  convenient  form  of 

good  accuracy.  Thus,  we  use 


COX0  + X0'1]^,  [Yn  + Yn“1l/2) 
fl([xn  + Xn-l|/2,  |Yn  + Tn‘L]/2) 

0 


(37) 


8 «Vl  + 

/ «Vi  + '„!/» 

as  the  most  accurate  representation. 

Last,  equation  ( 36  ) is  underdetermined.  We  want  an 
overdetermined  system,  so  as  to  be  able  to  use  a least  squares  solution 


for  the  vertical  deflections  and  gyro  biases.  Thus,  we  substitute 
for  £n  and  nn  by  the  form 


\£l  V k * Jl  bnk\ 


and 


n”  " cnk*k  +k-l  dnk  \ 


The  terms  with  n in  the  determination  of  C,  and  vice  versa,  enter 
because  the  deflections  are  strongly  cross  correlated.  The  exposition 
la  simplified  if  we  define 


r 5 C1 
2i-l 


r2i+2  5 n 


then  r' 


2N 

k-1  “ik  *k 


where  the  (e)  superscript  denotes  an  estimate.  We  would  like  to  minimize 
the  variance  of  the  estimation  error,  or 


P s (rt  - rj*5)2  - r2  - 2rirl(e)  + r{®>" 


, 2N  2N  2N 

•rf  - 2r  E a.,  * . + l Z a..a.-P.f, 

* i k*1  ^ k*1  !■]  ^ ^ 1 


To  minimize  F,  we  find 


- 0 - -2r.f.  +2?  a41 
«alk  Ik  H 11  kl 


Taking  expectation  values  of  both  terms  In  (38)  gives 


<riV  • A *u  <fkV 


The  covariances  are  matrices  and  we  can  solve  for  a,  and  have 


a - <r.  t > <f,V 
ik  11  1 k 


where  >”*  i*  the  Inverse  matrix  of  • To  specify 


<r21-lcJl-l>  " ^ > 


<r, 


2i-2*  r21-2 


> ■ 


<n  n 


jii  .jin  i,  „ ij  ium.m 


(39) 


The  needed  covariances  are  usually  estimated  using  the  assumption  of 
an  Isotropic,  homogeneous  covariance.  That  Is  the  power  spectrum  of 
gravity  anomaly  fluctuations  Is  a function  of  neither  position  nor 
direction.  Then  the  anomaly  covariance  Is  a function  only  of  the 
distance  between  two  points.  The  covariance  of  the  vertical  deflections 
can  be  derived  given  the  anomaly  covariance  (Shaw  jst  al.  1969) . 

A number  of  models  have  been  used  for  the  anomaly  covariance. 

If  ♦ (r)  is  the  anomaly  covariance,  then 

OO 

♦rr  " + (®in2e  ” cos26)  fc(r)] 

0 gg  » c 

♦ • o*  l ♦„„(*) /oa  + (cos2e  - 8ln2e)  f (r)J 

nn  o gg  » c 

•in  9 «■ 9 fc<r> 

2 

vhere  is  the  anomaly  variance,  og  «■  crg/gQ  ^2» 

r ■ (X2  ♦ Y2)/D  with  D the  correlation  length.  The  angle  6 

Is  tan_1(Y/X).  The  function  fc(r)  can  be  derived  once  #gg  is  known. 


A number  of  models  for  the  anomaly  have  been  used.  For  example. 


exponential,  Bessel,  (Shaw  et  al,  1969)  and  second  order  Markovian 
(Kasper,  1971)  models  give 

exponential 


*oa  - «xp  <-r) 
ss  s 

2 

f c(r)  - r2  - (1  + 2/r  + 2/r2)  e“r 


Bessel 


d2  r K (r) 
g 1 


£. (r)  - 4/r2  - 2K  (r) 

v O 


(4/r  + r)  Kj(r) 


2n<*  Order  Markovian 

*»  ‘ "l  *‘r  u * rl 

fL  ■ 6/r2  - e"r  (r  + 3 + 6/r  + 6/r2! 

thus,  <CAn^>  * -2o?  sin  6 cos  8 f (r  ) 

0 c 11 

where  8 - tan"1  I (Y1  - Y1)/  XX1  - X*)J 
tn  - D I O1  - X1)2  + (Y1  - Y1)2  ]1/2 


end  the  other  elements  can  be  filled  similarly 


I 


Rewriting  (36)  to  concentrate  on  the  un  and  vn  variables, 
we  have 


"51  t1 

t-o 


< ♦ 


y,k 


n-2  n-1  4 4 

z n r 1 P 

j-o  i-j+1  Ju  k 


v* 


(40) 


-n-1 


+ A0"1  T 


u k ^ 


where  the  [ ] ..  notation  means  the  i,  k'th  element  of  the  matrix 
Ik 


enclosed  in  the  bracket.  It  is  also  convenient  to  reqrlte  T as  a 
sum  of  vectors,  l.e. 


Tn  - a + «a°  + bn 


where 

0 

0 

r 

gC 

a • 

0 

, 6an  - 

0 

and  b”  - 

-gn11 

■ 

0 

0 

0 

a 

6on 

0 

r 

63n 

0 

Y 

• 

6rn 

J 

The  a vector  is  the  mean  gyro  drift  rates  over  the  mission,  and  the 
da  vector  is  the  deviations  of  the  drifts  from  the  average.  Since  we 
assume 


- r. 


2N 


2n-l  " k*l  *2n-l,krk 


"n  ■ r2n-2  - & *2n.2'k  • 


j .A 


n 2N 
bn  • E 


<r2n-lV  < W 


and  (40)  becomes 


<r2n-2rl>  <rlV 


{ l "ii1  »ll„  u 

1-0  v»k 

n-2  i aj  “n-1 

+ [ E n -r1  AJ  + An  1 ] a. 

i.ixi  y.k  *> 


J-0  i«j+l 

n-2  , n-1  < .4 

t r n 


J , ,n-l  , n-1 


+ E [ n Tx  AJ  Ju,  6 < + A * «a; 
J-0  i-J+1  v*  * Hk 


2N  n— 2 n— 1 i 

+ e [i  { n aJ)Uu 

ra-l  J-0  i-J+1  $k 


<r2J-lrl><rlrta> 


Wi^iV1 


'VtV'V.”'1 


> ' 


which  la  in  a form  suitable  for  least  squares  analysis.  The  terms 
Involving  6a  do  not  enter  Into  the  solution,  except  to  provide  an 
estimate  of  the  weighting  of  each  term  of  the  optimal  solution. 
However,  for  the  current  report  we  shall  use  even  weights  and  thus 
drop  those  terms  from  the  equation.  We  have  then 


.n  0 
_ - A W. 

v11  u k 


- Bn  a,  - Cn  r 

u v *■  u m “ 
y*K  v*® 


where  A,  B,  and  C are  defined  by  (32).  We  want  to  minimize 
m 2 


H * 

Z Z Fn 
n-1  u,v  ^ 


which  Implies 


77  - -2  Z Z F“  Bn 

n-1  u,v  « u>k 


6r_  - -2  E • I F°  Cn  f - 0 

“ n-1  u,v  v y ® 

which  gives  the  following  set  of  simultaneous  equations 
M - M - 


;n 

Bn  + r 

M 

Z Z 

Cn  Bn 

u . m 

v*  * 

n-1  u,v 

u u 

V*m  y» 

n 

(u 

vvn 

aU  0 v 

' Vi  ’ 

vA 

Bn 

£,k 

permuted 
over,  k 


- 


— 





and 

a 

1 


which  Chen  Rive  Che  deaired  gyro  races  and  defleccions  of  Che  verdcal. 

The  permuCaCion  over  p is  noc  carried  ouC  for  p»l»  2 or  p«2N,  2N-1. 
These  are  che  values  of  n and  K aC  Che  scare  and  finish  of  Che  mission 
and  are  known  beforehand. 


? Z Bn  c” 
n«l  u,v  u i u 


+ i ? z 

■ n«l  u,v 


cn  cn 

u _ u _ 
v*“  v*  P 


permuted 
over  p 


M 

• II 
n«l  u,v 


,un  An 
<yn  - A 


0 

1 V1 ) 
u 1 1 

V*1 


V’P 


?n,2 

2N,2N-1 


III.  Machine  Coded  Algorithm 

This  section  presents  a FORTRAN  source  listing  of  a coded  version 
of  the  algorithm  developed  In  section  II.  The  program  has  been 
debugged  to  the  extent  possible  without  actual  test  data.  Notes  on 
the  programming  are  contained  within  the  comment  cards  In  the  listing. 


Computer  printouts  not  necessary  for 
understanding  of  the  report. 


nnnonnrrnrnonronornnnponn  nnnonno 


*************************************************************************** 


PROGRAM  FOR  THE  OPTIMIZED*  POST-MISSION  DETERMINATION  Oh 
THt  DEFLECTION  OF  THfc  VtPflCAL  USING  RGSS  DATA 
PROGRAM  PRODUCED  BY  PHOENIX  CORPORATION 

*************************************************************************** 

COMMON  CPHI  ,SPHI , TPHI 
~ DIMENSION  MbO) 

DIMENSION  TOO (SI ) , TS?UP(S1 ) ,U(51 ) , V(50) ,XX(51 ) , YY(51) ,XHAT(51)  , 

2 YHATCSl ) »X(SO) ,Y(50) .START (6) .STEM (6) 

DIMENSION  P3I(S0,6,6)  ,XL(50,t>,to) ,DUMMY(6,t>)  ,DUM2(fe,6) ,DUM3(6,6) ♦ 

2DUM0 ( b ,6 ) 

DIMENSION  COEF(100,100),DEFL(100,100).COVAR(100  ,100), 

1 SNMATUOO,  100)  ,F  (100)  , VAR (100)  ,SCJMAT(  100) 

EQUIVALENCE  (SOMA  1(100,1) ,SUMAT(1)) 

DATA  G/9,806SSE2/ 


INPUT  DA 
N 1 = 


MSTOP 

TGU(I 

TSTOP 

U(I) 

V(l) 

XX(I) 

Y Y ( 1 ) 

XHAT, 


TLAT 
XIO  *E 


START 


« Oh  POINTS  AT  WHICH  A POSITION  IS  SPECIFIED 
THE  FIRST  AND  LAST  POINTS  BEING  THE  START  AND  STOP 
OF  THfc  VEHICLE  • THE  REMAINDER  THE  POINTS  AT  WHICH  THE 
OEFLECTION  IS  TO  HE  DETERMINED, 
a NUMBfcK  OF  VEHICLE  STUPS  IN  A GIVEN  MISSION 
) A TIME  SPENT  TRAVELLING  ON  I-TH  LEG 
(I)  a TIME  SPENT  STOPPED  ON  I-TH  LEG 
a X OR  NRTH  VELOCITY  ERROR  AT  END  OF  1-TH  LEG 
s EAST  OR  Y VfcLUCITY  ERROR  AT  END  OF  1-TH  LEG 
s NOHTH  POST! ION  AT  END  UF  I-TH  LEG 
a EAST  PUSITIUN  AT  E.mD  OF  1-TH  LEG 
YH A T s NORTH  AND  EAST  POSITIONS  AT  WHICH  THE 
DEFLECTION  IS  TO  HE  DETERMINED,  • EXCEPTIONS  ARE 
THE  FIRST  AND  LAST  VALUES  OF  XHAT.YHAT  WHICH  ARE 
THE  STARTING  PLACE  AND  FINAL  STUPPING  PLACE  OF  THE 
MISSION 

a TERRESTRIAL  LATITUDE  OF  THE  MISSION 
TA0,X1F IN,tTAF]N  a THE  VALUES  OF  THE  NORTH  ANDEAST 
DEFLECTIONS  OF  THE  VERTICAL  at  THE  START  AND  STOP  UF 
THE  MISSION  RESPECTIVELY 

(I)  = INITIAL  CONDITION  VECTOR,  CONTAINS  IN  SEQUENCE' 
G*XI0,G*ETA0,0, ,0. ,£TAO,XIO 


READ(S.l)  MSTOP, N1 

1 FORMAT (215) 

MS  = MSTOP  -h  1 

READ (5, 2) (K ( I ) , TGO( I ) , TS TOP ( I ) ,U(I),V(I) ,XX(I) , YY < I ) ,1*1, MS) 

2 WRlTE(e!lfll)ll5?GO(I),TSTOP(I),U(I),V(I),XX(I) ,YY(I) ,I*1,M$) 
101  F0RKAT(1X,IS,6(3X,E1Q,4)) 


REAU(5,2)(K(I) ,XHAT(I) ,YHAT(I) .lal.Nl) 
wRITE(6,102)(K(I).XHAf  (I),YHATU),I*1,N1) 
102  FORMATUHl  ,(!■>, 2Ela, U)) 

READ(5,3)TLAT,X(IU.ETA0,XIFIN,ETAFIN 

READ(5,i)(START(I).I=l,b) 

3 FORMAT ( 6E12.U) 


rnooonnonnoncnonnnnnnn  onnnnrnn  ooo  nnn 


WTT 


fcPm(b,10i)MSTnP,Nl  , ( START  U)  ,Iai  , 6 ) , TL  AT  , XI 0 ,E  T AO  , XIF  IN  ,E  T AF  J N 
103  format umo,2IB/2Ux  ,6tia#a/)) 

SHHl  = SlN(TLAT) 

CPHi  s COSULAI  ) 

TPHI  a SPHI/CPHI 

INITIALIZE  1 HE  TIMfc  ADVANCEMENT  OPERATORS 
- CALL  AOVANS 

FIND  THE  CENTERED  VEHICLE  POSITION  FOR  EACH  TIME  STEP 

DO  200  1 s 1 .MSTOP 

XU)  s tXX(lJl)  ♦ XX(I))/2, 

200  YU)  s (YYU  + 1)  ♦ YYU))/2, 

DEFINE  THE  TIME  SHIFT  OPERATOKS  FOR  EACH  TIME  STEP 

THE  PSI  OPERATOR  GIVES  THE  CONTRIBUTION  COMING  FROM 
THE  VALUES  OF  THE  VARIABLES  THEMSELVES  AT  THE 
BEGINNING  UF  THE  TIME  STEP 

THE  XL  OPERATOR  GIVES  THE  CONTRIBOTION  FROM  THE 
DRIVING  TERMS 

DU  300  I a l,MSTOP 

CALL  TIME(TGOU)  fTSTUP(I)  ,PSI,XL,N,X,Y) 

300  CONTINUE 

DETERMINATION  UF  THE  COEFFICIENT  MATRIX  OF  THE  SET  OF  LINEAR 
EQUATIONS  CONTAINING  THE  UNKNOWN  NOAM  1 TIES* 

CUEFI*,2*N1)  CONTAINS  THE  CUNSTANT  TERM 
CUbF(*,2*Nl-i)  I » 3,2,1  CONTAINS  THE  TERMS  INVOLVING 
COEF ( * , J ) J * 1 - 2*N1  - A CONTAINS  THE  TERMS  INVOLVING 
ALPHA, BETA .GAMMA,  RESP.  THE  GYRO  DRIFT  RATES  ABOUT 
VERTICAL, NORTH.  EAST  AXtS,  RESP, 

THE  XI  : ETA  VALUES  AT  THE  CHOSEN  POINTS 


INITIALIZATION  UF  THE  CONSTANT  PART  OF  THE  COEF  MATRIX* 

THE  PSI  UPERATUR  IS  SUCCESSIVELY  OPERATED  ON  THE 
INITIAL  CONDITION  VECTOR,  START,  TO  PPUDUCE  THF 
EFFECT  OF  THE  INITIAL  CONDITIONS  AT  THE  START  OF  THF. 
MISSION  ON  THt  U AND  V EkkOR  VELOCITIES,  THE  DIFFERENCE 
BETWEEN  THESE  QUANTITIES  FORMS  PART  OF  THE  TERM  INVULV- 
BETWEEN  THESE  UUANTITIES  RURMS  PART  UF  THt  CUNSTANT 
TERM,  A CONTRIBUTION  Out  TU  THt  DEFLECTION  OF  THt  VERTICA 
TERM,  A CUN  TR I PUT  J.ON  DUE  TU  THE  DEFLECTION  OF  THE 
OF  THE  VERTICAL  IS  SUBTRACTED  LATER, 

DO  1000  I s l.MSTQP 
CALL  ENUIVU  , PSI  , DUMMY) 

DO  1010  kks  l,e»  . . 

STEM(kk)sO, 

DO  1010  L = 1,0 
1010  STEM(kk)  s STtM(KK)  ♦ DUMMY(KK,L)  * START(L) 

DO  1020  L s 1 ,o 

1020  STAR T ( L ) = S T E M ( L ) 

COEF  (2*1-1, N2)  a U(I)  * STARTU) 


nonnn  non  mono  ooooororono 


COEF  (2*1  ,N2)  = V(I) 
1000  CUiMTINUt 


START(2) 


DETERMINATION  OF  THE  TERMS  in  THE  COEF  MATRIX  WHICH  DEPEND 
UN  THE  DEFLECTIONS  OE  TH t VERTICAL 


FIRST  STEP'  DEE  INF.  A MATRIX  - DEFL  - WHICH  GIVES  THE  VARA 

FIRST  STEP'  DEFINE  A MATRIX  - OEF'L  • WHICH  GIVES  THE  DEPENDENCE 

UF  THE  VELOCITY  ERRORS  ON  THE  VALUES  UF  THE 
OEFLEt  T IONS  AT  EACH  OF  THE  MIDPOINTS  OF  EACH  TRAVFL  LR 

OEFLECTIONS  at  EACH  OF  THE  MIDPOINTS  OF  EACH  TRAVEL  LEG 


mo 

1100 


DO  1100  I s l.MSTOP 
CALL  EUUIVU,  XL,  DUMMY) 
DEFL(2*I-J ,2*1-1)  = DUMMY (1,1) 


DEF  L ( 2* 1- 1 ,2*1 
0EFL(2*I ,2*1-1 
DF.FL(2*I,2*I)  = 
IF  U,EU.MST0P) 


I! 


6S 


s DUMMY (1,2) 
* DUMMY (2.1) 
DUMMY (2,2) 

GU  TO  1101 


v * • 

K a I ♦ I 
DO  1110  N s KK.MSTOP 
CALL  EUUIV(N,PSI fDUM2) 

CALL  XMAT (OUM2, DUMMY ,DUM3) 
CALL  EUUAL(DUM3, DUMMY) 
DEFL(2*N-l ,2*1-1)  s DUMMY ( 1 , 
DEFL(2*N-1 ,2*1)  s DUMMY! 1,2) 
0EFL(2*N, 2*1-1)  c DUMMY (1,1) 
DEFL  ( 2*N , 2*  I ) = Dl'MMY(2,2) 
CONTINUE 
CONTINUE 


1) 


SECOND  STEP'  DEFINE  THE  MATRIX  - COVAR  - WHICH  , WHEN 

MULTIPLIED  BY  DEFL  GIVES  THt  DEPENDENCE  OF  THE  COEF 
MATRIX  UN  THE  OEFLECTIONS  Al  THE  DESIRED  POINTS 

CALL  COLLUC(MSTCtP,Nl , COVAR, X,Y,XHAT,YHAT) 

N2P  * N2  - a 


THIRD  STEP'  DO  THE  MULTIPLICATION 


U50 


DO 

DO 


1150 

1150 


I 

J 


£UEF ( I,J) 


COEf1u.A 


DO 


r \ i , \ 
1160 


1 3 


l, M2 
1.N2P 

0, 

1 ,F2 

CUtF ( I , J ) 

t,M2 


♦ DEFL ( I ,L)  * COVARfL, J*2) 


UPDATE  THE  CONSTANT  PART  OF  THE  COEF  MATRIX  TO  ACCOUNT  FOR  THE 
•KNOWN  VALUES  OF  THfc  DEFLECTION  AT  THE  START  AND  STOP 
OF  THE  MISSION 


G * XIO  * OEF'L ( I ,L)  * COVAR(L  , 1 ) 

G * FT  AO*  DEFLU, L)  * CUVAR(L,2) 

G *XIFIN*  DEFLU, L)  * CUVAK  (L  ,N2- 1 ) 
G *E1AFIN*DEFLU,L)*C0VAM(L,N2) 


nnnnnn  noonno  o o rtnrnn 


F 


1155  CONTINUE 
1160  CUNTlNUt 


FILLING  IN  1 HOSt  PARTS  UF  THE  COEF  MATRIX  THAT  DEPEND  UN  THE 
GYRU  DRIFT ,RAIES 

DU  1210  N a 1,3 
- UU  1210  I = 1,2 
1210  COEF (I ,N2-4+Nl  a XL ( 1 , I , 3+N) 

CALL  EUUIVC1 , XL, DUMMY) 

OU  1200  I s 2.MSTUP 
CALL  EUUIVd  ,PSI  ,UUM2) 

CALL  EUUl V C I , XL , UUM3 ) 

CALL  XMAI (UUM2, DUMMY, OUM4) 

CALL  AUDIDUMa ,DUM3, DUMMY) 

DU  1220  N s 1,1 

COEF ( 2 * I • 1 «N2  • 4 ♦ N)  ■ DUMMY (1,3  ♦ N) 

1220  CUtF(2*I ,N2-«tN)  * DUMMY l 2 • 3 + N ) 

1200  CONTINUE 

N2M  s N2  • 1 


1300 


1400 


1450 


C 

C 


DEFINING  THE  MATRIX  - SQMAT  - WHICH  SOLVED  GIVES  THE  DESIRED 
LEAST  SUUAKES  SOLUTION  EUR  THE  DEFLECTIONS  OF  THE 
VERTICAL  AND  THE  GYRO  OKIFT  RATES, 


DO  1300  I a 1 ,N2M 
DU  1300  J a 1 ,N2 
SOMA T ( I , J ) a 0, 

DO  1300  N a 1.M2 

SOM AT C I , J ) a SUMAT(IfJ) 


♦ CUEN (K , I ) * COEF (N , J ) 


AFTER  SUBROUTINE  SOLVE.  SGMAT(*,2*N)  CONTAINS  THE  SQLUTIUN 
VECTOR'  THE  LAST  THREE  ARE  THE  GYRO  RATES  AND  THE 
REST  ARE  THE  DEFLECTIONS  OF  THE 
THE  LAST  COLUMN  IS  EUUIVALENCED 

CALL  SOLVE (SNMAT) 

DO  j40°_i  = l, M2 


VERTICAL, 

TO  A VECTOR  SUMAT 


F ( I J so. 

DETERMINATION 
J = 


1 ,N2M 

♦ COEF ( I , J) 


DO  1400 

MI)  * F C 1 ) 

DO  1450  I = 1.M2 

VAR ( 1 ) « (F(I)  - COEF ( I ,N2) ) **2 

SUMSU  * xSUMSU  ♦ VAR  f I ) 

SIGMA  a SURUSUMSG/FLUATCM2-D) 
NN  a N2  - 4 
DO  1460  I a 1,NN 
SUMATU)  » SUMAT  Cl) /G 


OF  THE  ACTUAL  VARIANCE  OF  THE  SOLUTION 
* SQMAT ( J) 


9 


OUTPUT  THE  FINAL  RESULTS 

[TE (6*110)  SUMAT(NM+n.SOMAT(NN*a)  ,SOMAT(NN*3) 


WRITE (6*110)  SUMAT{NN+1) .S0MAT(MN*a) *SQMAT(NN+3) 
no  FORMAT  l lm  . • F INAL  RESULTS  •//•»*VHU  DRIFT  RATES,  ALPHA  * ,£  1 2 , a , SX 
2, 'SETA' ,E12.a,bX, 'DAMMa* ,hl2.a//'UEFLFC TIUNS  UF  VtRT*/'  XI' 

3 ,17X,»fcTA', 1 }x 'NORTH 'PUS' , 11 X, 'EAST  PUS*//) 

WRlTt(t>,  1 1 1 ) (SUM  A TCI) . SUMATd  + t ) , XHAT  ( 1 ) . THAT  ( I ) , 1 = 1 ,N2 ,2) 

- WR) TElh.l  12)3UmSU,SIGMA,(I  ,VAH(]  ,1  s 1,M2)  . • uj 

112  FURMAU  1H1 VARIANCE  OF  SOLUTION  * ,fcl2.4.bX  , 'SIGMA»  ,E12.«/ 

• »INUIVIDUAL.SD.kRH0RS'/(lX,I3,SX,tl2.a)) 


STOP 

END 


[ \ 
I : 


croon  oooo  noon  ooooooo  nnnnncn 


T 


SUBROUTINE  C0LLUCCM,N1 ,CUVAR , X , Y , XHA T , YHA T) 

THIS  SUBROUTINE  PRODUCES  A MATRIX  - COVAR  - THAT  PWODUCtS 

VALUES  ROW  THh  DEFLECTION  OF  THt  VERTICAL  AT  POINTS*  I, 
FROM  THt  VALUES  OF  THt  DEFLECTIONS  AT  OTHER  POlNlS,  J, 

THIS  IS  OUNt  BY  STATISTICAL  CULLUCATIUN. 

FOR  A DERIVATION  OF  THt  METHOD  SEE  THt  PHUtNIX  CORP.  REPORT 

- DIMENSION  CV1(50.50).CVINV(50,50),CV2(50,50),COVAR<50,50), 

2 X(50),Y(50),XHAT(50)  ,YHAT(50) 

SIGG2  = SIGMA( I , ) 

SIG02  = SlGD  U.) 

DETERMINE  the  COVARIANCES  between  THt  DEFLECTIONS  AT  THE 
BASIS  POINTS.  ODD  INICtS  DENOTE  XI  VALUES.  EVEN 
INDICES  ETA  VALUES,  , THE  LU VAR  I ANCES  ARt  OERlVtD 
UNDER  THE  ASSUMPTION  UF  ISOTROPIC,  HUMuGENEUUS 
COVARIANCE  UF  THt  GRAVITY  ANOMALY, 

DO  500  I a !,NI 
DO  500  J = t.Nl 

R c SORT ( ( XHA  III)  »XHAT ( J£E**2  4 (YHAT(I)  - YHAT(J))**2) 

STH  = CYHATC1)  • YHAT ( J ) ) /R 
CTH  b (XHAT(l)  * XHAT(J))/R 

CVI (?*I-i ,2*J-i)  s SIG02  * ( PH  I GG ( R ) /SIGG2  4 (STH**2-CTH**2) *FC(R) ) 
CVi (2*1  ,2*.D  = SIGD2*tPHIGG(R)/SIGG2+(CTH**2-  STH**2) *FC (K) ) 

CVI (2*1-1 ,2*J)s  -2.  * Si GO 2 * STH  * CTH  * FC(R) 

CV1(2*1,2*J-U  = CVI  (2*1-1  ,2*J) 

500  CUNTINDt 


INVERSION  OF  THE  CVI  MATRIX  IS  FOUND  IN  CVINV 


CALL  MATINVtCVl , CVINV) 
DO  000  K s l,M 


DETERMINATION  OF  THE  COVARIANCES  BETWEEN  THE  BASIS  SET  AND 
THt  SET  OF  POINTS  DETERMINED  BY  THE  MISSION  LEGS. 


DO  600  I * 1 ,N1 

R c SURTKXCK)  - XhAT(l))**2  4 C Y (K ) -YHAT  ( I ) )**2) 

STH  b(Y(k)  - YHATCin/R 
CTH  = ( X ( K ) - XHAT(I))/R 

CV2(2*K-l  ,2*1-1)  s SIGU2  * (PHI GG (R) /SIGG2  4(STH**2-  CTH**2) *FC (R) ) 
CV2(2*K-(,I*1)  = -2,*S1GD2*STM*CTH*FC(K) 

CV2(2*«, 1*1.1)  s CV2(2*K-1  ,2*1) 

600  CONTINUE 
N2  * 2*N1 
M2  s g*M 

PRODUCTION  OF  THE  COVAR  MATRIX  BY  MULTIPLICATION  OF  CV2  BY 
. CVINV 


DO  700  L s 
DO  700  * * 
COVAR(L.K) 
DO  700  I a 


1 ,M2 
1 ,N2 
® Of 
1 fN2 


rnnnnonn 


V 


m 


f 


"T3 


T 


700  C(JVAH(L,K)  s C0VAR(L,K)  ♦ C V 2 ( l , I ) * C V I N V ('  I , K ) 

RETURN 

two 

F UNC  TION  SIGMA(X) 

THIS  FUNCTION  AND  ITS  ENTRIES  GIVE  THE  NECESSARY  VALUES  F OK 
THt  COMPU  T A T I ON  UF  THE  UtFLECT  IUN  CUV  AH]  ANCES. 
CUHRtNTLY.  1 Ht  FUNC  f IUN  ASSUMES  A SECOND  ORDER  M AKKU V IAN 
STRUCTURE  *-UR  THE  ANOMALY  COVARIANCE.  VAR  IS  THt 
VARIANCE  OF  THE  ANOMALY  ANQ  D IS  l HE  CORRELATION 
LENUTH 


8 


8 


DATA  VAR,D,G,ROOT2/3,3bE-2,«E6,9,B0b65E2,l,«1421/ 

SIGMA  5 VAR 
RETURN 

ENTRY  SIGO(X) 

SIGO  s VAK/G/ROOT2 
RETURN 

ENTRY  PHIGG(R) 

0 s K/U 

PHIGG  a VAR*EXP(-U)*(1,+G) 

RETURN 
ENTRY  FC(R) 

0 = R/D 

FC  * b,/Q**2  • EXP(-Q)  * (U  + 3,  ♦ 6,/Q**2) 

RETURN 

END 

SUBROUTINE  AOVANS 

THIS  SUBROUTINE  PROVIDES  THE  VALUES  OF  THE  NECESSARY  TIME 
SHIFT  MATRICES, 

THIS  ENTRY  INITIALIZES  THE  VALUES  OF  THE  NECESSARY 
EIGENVECTOR  MATRICES 


COMMON  CPHI.SPHI.TPHI 
COMPLEX  R(o),P(2J,C(6T.C2(to) 


2 TINV(3, 3;  .PH(bf  6)  .PHI  (o,6) , VL (6,6) . VI  l , PO ( 3 . 3 ) , VP ( 3 , . 

DIMENSION  RPHUb.oJ  ,RVLI(b,6)  , RPG  C 3 * 3 > . K VP  l 3 , 3 ) , RPHO  lb  ,6 ) , 
2 DUMMY (6,6) ,DUM2(6,bJ ,PSI(5Q«b,6)iXL(50,b,b) 

DATA  G,KtAKlH,UMtGA/9.00toto5E2,b,3710  3tcJ,7.29212E-5/ 

OMEGAS  = SORT ( G/Rb AR f H ) 

DO  200  I = 1 ,b 
DO  200  J s l,o 
200  RPHU(I.J)  a 0, 

DO  210  I = 1,3 
210  RPHOCl,!)  = 1. 

FIRST  FOR  THE  TIME  WHEN  THE  VEHICLE  IS  IN  MOTION 


R CONTAINS  THE  ROOTS  OF 


THE  SECULAR  EQUATION 


R( 1 ) * (0,,1,)  * OMEGAS 
R(2)  e-R(l) 

A 1 a OMEGA* *2  ♦ 0MEGAS**2 

B1  a SORT (UMtGA**u  + U”tGAS**««-2  , *UMEGA**2*UMEGAS**2*  (1 ,+2,*CPHI2) ) 
R(3)  a (0,,1)*SQRT(0,5  * AI  ♦ 0,5  * Bl) 


a 


H(<n  * • h(3) 

R(S)  S (l,,o.)*SURT(0.5*bl*0.5*Al) 

H(6)  » -R(S) 

FILL  IN  THE  VALUES  FOR  THE  TRANSFORMATION  MATRIX , S.  AND 
ITS  INVtRSt,  SINV 

I = 


DO  500  _ 

A s (H<J)**2 

be2,/HU) 

2 ♦ « 


SCI.!)  » 

ill:!!  i 


1 ,6 

‘ + O^EG AS**2) 
n*A**?*(K(I)**a  ♦ (<JMEGA*UME&AS*CPHl)**2) 
* R C I ) * * 3 * (OMEGA*DwtGAS*SPHl ) **? 


Hi 


,«)  = 
i(I,b)  * 
S(I,b)  = 
SlNvtl , I ) 
SINV(?,I) 
SINVC 3. 1 ) 
S1NVCO.I ) 
SINVC5.1) 
500  SINV(6,l) 


A/H/REARTM*{ (U^EGA*CHhI )**?  . R(I)**?  ) 

2,*R(  1 )**3*UWfeG A* SPHI/H/Rf  ARTH 
(UMEGA*CPHl)**?  * A*  R ( I ) /b/REARTH 
-UMtG  A * CPHI  * H(I)**2*A/H 
OM£GA*SPHI*R( I )**?*(R( I)**2-OMtGAS**2)/B 
A/H  * R ( 1 ) ** 3 
s U*A/R(I) 

S G*Ur-’hGA*SPHl 
a G*A/k (!)**? 

* -(A**?  ♦ (R(I)*OMEGA*SPHl)**2)/(R(I)*OMEGA*CPHI) 

* .HU)  * OMEGA* SPH I 
s A 


NOW  DO  THE  SAME  FOR  fcHEN  THE  VEHICLE  IS  STOPPED 


HU  ; UMe“ 


1 • /SQK  T ( 2 1 ) 

) * -CPHI  * A 1 
) a SPHI  * A 1 
) * A 1 


H s lji:i) 

!)  ‘ 1‘rt!  3) 
1)  c SPHI 

is  i r1 
!!:.u 


r 


TINVC1,?) 
TINVl 1 , 3 
TINV(?,1 
TINVC?,? 


c 

8 


-CPHI  * 

JWl” 

SPHI  * AJ 


A 1 


c 

c 

c 


c 

c 


. _ , . v TINV(2f 1 

TINVC?, 3)  a CPHI 
TINV(3,1)  = -10. ,1.)  * A I 
TINV(3,2)  a -TlNV(3,t) 

TINV13.3)  a 0 

ENTRY  TlMfc(Tl ,T?,PSl,XLfN,XPOSfYPOS) 

THIS  ENTRY  ACTUALLY  CALCULATES  THE  TIME  SHIFT  MATRICES 
T 1 IS  THt  TIME  THE  VEHICLE  IS  MOVING' 

72  IS  THfc  TIME  STOPPED 

THE  DERIVATION  OF  THE  VARIOUS  OPERATIONS  PERFORMED 
HERE  IS  FOUND  IN  THE  PHOENIX  CURP,  REPORT 

T « Ti  * 0.5 
DO  600  K * 1,6 


C(U  * CEXP(H(K)*T) 
600  L2(K)  a C (L ) * UK) 
DO  700  I a 1,6 
0U  700  J a 1,6  v 
PH  ( 1 , J ) * C‘t.,0.) 


muiui  ■*  v 

PHI(I.J)  3(0, ,0.) 

VL(I.J)  s (0.,0.J 
VLI(l.J)  = (0,,0f) 

PHl(I,J)  = PH1(I,J)  ♦ C2(K)*SINV(I,K)*9(K,J) 

PH( ) * PH(i,jJ  ♦ R (K ) * COO*SINV(l,K)*S(K,J) 


VLIU.J)  » VLI(I.J)  ♦ (C2(K)«U,  ,0,))/R(K)*SINV(I»K)t5(KtJ) 
700  VLU.J)  a VL  ( I « J ) ♦ ((K)*SINVUtlO*S(KtJ)) 

do  poo  i s i , <) 

“ ■ “ PH ( 6 , I ) * ( YPOS(N+ 1 ) • YPDS  ( N ) ) 

PH(a,I)*(XP(3S(Ntl)»XPCJS(N)  ) 


1,6 

s Phl( J,I) 

* PH l ( 2 , 1 ) 

* Vll(l,I) 
= VLI(2,I) 
a 1,6 

00  610  J=1  ,6 


PH 1(1,1) 
PHI ( 2 , 1 ) 
VL 1 (T  « I ) 
800  VL I ( 2 , 1 ) 
DO  610  I 


VL(U,I) 
VL(«,1 ) 


* ( YPOS ( N+  1 ) ’ - YPOS(N) ) 

* { XPUS( N+ 1 ) - XPOS(N)) 


RPHI ( I * J ) 3 REAL (PHI ( I , J) ) 
610  R VL  I ( 1 , J ) s KfcAUVLJUtJ)) 


DC1)  s CfcXP(P(l)  * T2 ) 
0(2)sCfcXP(P(2)  * T2) 

00  900  1 3 1,3 
00  900  J 3 1,4 
P0(I,J)  a 0(1)  * TINV(Itl)*r(J ,J) 


2 ♦ T1NV(I,3)  * T ( 3 , J ) 

900  VP(I.J)  * (0(1)-1.  )/P(l)*TlNVU,l)*T(l,J) 
2 ♦<0(2)-l.)  / P(2 J * TINV(I,2)  * T(2,si)  + 


♦ 0(2)  * TINV(I,2)*T(2,J) 


8 


1,3 
1,3 

KEAL(PO(I,JJ J 


)0  910  I 

)0  910  J 

KPO(I,J) 

910  RVPC1.J)  * Rt AL ( VP ( I 
00  920  1*1,3 
RPHO(3+I ,3)  s -OHtGA*CPHI/REARTH*RVPI(I,i) 
DO  920  J * 1 .3 
RVPO(3>I ,3+J)  a RVP ( I , J ) 

920  HPHU(3+1 ,3+J)  * RPU ( I , J ) 

CALL  XMA1 (RPHI .NPHU.DUMHYJ 
CALL  fcUUI (N.PSl ,0LMMY) 

00  950  I » 1 i 6 

00  950  J = 1,2 

950  PSI ( N , J . J ) s 0, 

CALL  XMAT(HPMl,RVPOfOO«HMY) 

CALL  AUUtRVLl ,0UyMY,UUH2) 

CALL  twUl(NfXL,D0H2) 

HFTURN 

END 


T2  * TINV(I ,3)  * 


TC3, 


nnonr 


mniiimiLjiii  juih  ii  ni.u^w>ipp 


f 


HANOY  MATRIX  OPERATIONS  NUT  FOUND  IN  FORTRAN 
fcUUiV  MAKES  A SMALL-  SEC 

fcNUlV  SETS  A SMALL  bXb  MATRIX  EQUAL  TO  A SECTION  A LARGER 
ONE  , LQUI  IS  THE  INVERSt 

ADO  ADOS  TaU  MATRICES,  XMaT  MULUPIIES  TWO  MATRICES  AND 
EQUAL  SETS  ONE  MATRIX  EQUAL  TU  ANOTHER 

SUBROUTINE  EQUI V ( N , A , B ) 

DIMENSION  A(5Q,6,6) ,B(b,6) 

DO  200  I s 1,6 
DO  200  J * t ,6 
200  B ( I , J ) a A(N,1,J) 

RETURN 

END 

SUBROUTINE  XMAT(A.B,C) 

DIMENSIUN  A(b,6) ,6(6,6) ,C(6,6) 

DO  200  I a 1,6 
DO  200  J = 1,6 
C(I,J)  = 0, 

DO  200  K a 1,6. 

200  C(1,J)  = C(I,J)  ♦ A ( I , K ) * B(K,J) 

RETURN 

END 


SUBROUTINE  EQUAL ( A , B ) 
DIMENSIUN  A ( 6 , 6 ) , H ( 6 , b ) 
DO  200  I a 1,6 
DO  200  J » 1 ,b 
200  B ( 1 , J ) * A ( I , J ) 

RETURN 

END 


SUBROUTINE  A0D(A,B.C) 

DIMENSION  A ( b , b ) , B ( 6 , 6)  , C ( 6 , 6 ) 
DO  200  I = 1,6 
DO  200  J s 1,6 

200  C(I,J)  * A ( I , J ) ♦ B(I,*/) 

RETURN 

END 


SUBROUTINE  EQU1(N,A,B) 
DIMENSIUN  A(bO,b,b) ,B(6,6) 
DO  2^0  I * 1,6 
DO  200  J = 1,6 
200  A(N,I,J)  a BU,J) 

RETURN 

END 


; 

- 
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